;
; Copyright (c) Microsoft Corporation.  All rights reserved.
;
;
; Use of this sample source code is subject to the terms of the Microsoft
; license agreement under which you licensed this sample source code. If
; you did not accept the terms of the license agreement, you are not
; authorized to use this sample source code. For the terms of the license,
; please see the license agreement between you and Microsoft or, if applicable,
; see the LICENSE.RTF on your install media or the root of your tools installation.
; THE SAMPLE SOURCE CODE IS PROVIDED "AS IS", WITH NO WARRANTIES OR INDEMNITIES.
;
;
; (C) Copyright 2006 Marvell International Ltd.
; All Rights Reserved
;
;******************************************************************************
;
; INTEL CONFIDENTIAL
; Copyright 2002-2003 Intel Corporation All Rights Reserved.
;
; The source code contained or described herein and all documents
; related to the source code (Material) are owned by Intel Corporation
; or its suppliers or licensors.  Title to the Material remains with
; Intel Corporation or its suppliers and licensors. The Material contains
; trade secrets and proprietary and confidential information of Intel
; or its suppliers and licensors. The Material is protected by worldwide
; copyright and trade secret laws and treaty provisions. No part of the
; Material may be used, copied, reproduced, modified, published, uploaded,
; posted, transmitted, distributed, or disclosed in any way without Intel's
; prior express written permission.
;
; No license under any patent, copyright, trade secret or other intellectual
; property right is granted to or conferred upon you by disclosure or
; delivery of the Materials, either expressly, by implication, inducement,
; estoppel or otherwise. Any license under such intellectual property rights
; must be express and approved by Intel in writing.
;
;
;*********************************************************************************
;
;  FILENAME:       xllp_WMMX_Regs.s
;
;  PURPOSE:        Provides assembly level code to support thread level
;                  context saving and restoration for the
;                  PXA processor's WMMX registers.
;
;  LAST MODIFIED:  11-Nov-2003
;******************************************************************************
;
; NOTES:
;
;    None.
;
;******************************************************************************

        AREA    |.text|, CODE, READONLY, ALIGN=5        ; Align =5 required for "ALIGN 32" to work
;
; List of Low Level Init functions in this source code include:
;
        EXPORT Xllp_Read_CoProc_Access
        EXPORT Xllp_Set_CoProc_Access
        EXPORT Xllp_Save_WMMX_Regs
        EXPORT Xllp_Restore_WMMX_Regs
        EXPORT Xllp_Store_All_WMMX_Regs
        EXPORT Xllp_Restore_All_WMMX_Regs


;******************************************************************************
;
;       ***************************
;       *                         *
;       * Xllp_Read_CoProc_Access *
;       *                         *
;       ***************************
;
; This routine is used to return the coprocessor access bits located in
; coprocessor 15, register 15.  It returns it to the location specified in
; r0
;
;******************************************************************************
Xllp_Read_CoProc_Access  FUNCTION

        stmdb   sp!, {r0 - r2, r14}

        mrc     p15, 0, r1, c15, c1, 0    ;Get Reg15 of CP15 for Access to CP0

        ;CPWAIT  r2                       ;Now 'stall' so the value can have time to be read
        MRC     P15, 0, r2, C2, C0, 0     ; arbitrary read of CP15
        MOV     r2, r2                    ; wait for it (foward dependency)
        SUB     PC, PC, #4                ; branch to next instruction

        str  r1, [r0]
        ldmia   sp!, {r0 - r2, r14}

        IF Interworking :LOR: Thumbing
            bx  lr
        ELSE
            mov  pc, lr          ; return
        ENDIF ; IF Interworking :LOR: Thumbing

        ENDFUNC


;******************************************************************************
;
;       ***************************
;       *                         *
;       * Xllp_Set_CoProc_Access  *
;       *                         *
;       ***************************
;
;
; This routine enables the access bits to Coprocessors 0 & 1
;
; Uses R0, R1. R2
;
;******************************************************************************
Xllp_Set_CoProc_Access  FUNCTION

        stmdb   sp!, {r0 - r2, r14}

        mrc     p15, 0, r0, c15, c1, 0    ;Get Reg15 of CP15 for Access to CP0
        mov     r1, #0x3                  ;Load R2 with mask for setting lowest bit
        orr     r2, r0, r1                ;OR current value with 1 to set the lowest bit
        mcr     p15, 0, r2, c15, c1, 0    ;Now set the value back into R15 of CP15

        ;CPWAIT  r0                       ;Now 'stall' so the value can have time to be written
        MRC     P15, 0, r0, C2, C0, 0     ; arbitrary read of CP15
        MOV     r0, r0                    ; wait for it (foward dependency)
        SUB     PC, PC, #4                ; branch to next instruction

        ldmia   sp!, {r0 - r2, r14}

        IF Interworking :LOR: Thumbing
            bx  lr
        ELSE
            mov  pc, lr          ; return
        ENDIF ; IF Interworking :LOR: Thumbing

        ENDFUNC


;******************************************************************************
;
;       ***************************
;       *                         *
;       * Xllp_Save_WMMX_Regs     *
;       *                         *
;       ***************************
;
;
; This routine is expected to be the entry point for the "SaveAll".  It checks
; the CUP and MUP bits in the control register to see if we need to save, if it
; is unnecessary, then we simply dump out of the routine.  If it is necessary,
; it sets a flag at the start of the save area and calls "SaveAll" to save all
; the WMMX registers.
;
; Uses r0, - R0 contains a pointer to the alloced memory buffer of 8 bytes long
;
;******************************************************************************
Xllp_Save_WMMX_Regs   FUNCTION

        stmdb   sp!, {r0 - r4, r14}         ;Store registers so we don't stomp anything (INC LR)

        mov     r4, r0                    ;Save R0 to R4, so pointer value doesn't get killed by CPWAIT

        tmrc    r1, wC1                   ;Grab the CUP & MUP bits in CP1, Reg1

        ;CPWAIT  r0                       ;Now 'stall' so the value can have time to be written
        MRC     P15, 0, r0, C2, C0, 0     ; arbitrary read of CP15
        MOV     r0, r0                    ; wait for it (foward dependency)
        SUB     PC, PC, #4                ; branch to next instruction

        ands    r1, r1, #0x3              ;We only are concerned with the lowest 2 bits

        ;if flag == 0 then no change since last save, skip save functionality
        beq  Finish_Save

        mov     r0, #0x1                  ;Set a flag in the memory area indicating we have actually SAVED
        str     r0, [r4], #4              ;Save & increment pointer

        ; Now branch to the 'save' function, R0 = pointer to save area
        mov     r0, r4
        bl      Xllp_Store_All_WMMX_Regs

Finish_Save
        ldmia   sp!, {r0 - r4, r14}         ;Now restore the regsiters we stacked3

        IF Interworking :LOR: Thumbing
            bx  lr
        ELSE
            mov  pc, lr          ; return
        ENDIF ; IF Interworking :LOR: Thumbing


        ENDFUNC

;******************************************************************************
;
;       ***************************
;       *                         *
;       * Xllp_Restore_WMMX_Regs  *
;       *                         *
;       ***************************
;
; This function is used to restore the WMMX registers.  It checks the flags saved in
; the beginning of the saved area to see if a restore is necessary.  If it is unnecessary
; the routine dumps out, otherwise it calls the "RestoreAll" function.
;
;       Uses r0 - pointer to the memory buffer containing the WMMX saved area
;
;******************************************************************************
Xllp_Restore_WMMX_Regs   FUNCTION

        stmdb   sp!, {r0 - r4, r14}         ;stack the registers used, so don't munge anything

        ldr     r1, [r0], #4              ;Load the first 4 bytes containing 'if saving' flag
        cmp     r1, #0
        beq     Skip_Restore

        ;Branch to the 'Restore' Function, R0= pointer to save/restore memory area
        bl      Xllp_Restore_All_WMMX_Regs

Skip_Restore
        ldmia     sp!, {r0 - r4, r14}

        IF Interworking :LOR: Thumbing
            bx  lr
        ELSE
            mov  pc, lr          ; return
        ENDIF ; IF Interworking :LOR: Thumbing


        ENDFUNC


;******************************************************************************
;
;       *******************************
;       *                             *
;       * Xllp_Store_All_WMMX_Regs    *
;       *                             *
;       *******************************
;
;
;
; Stores all WMMX Registers as listed below
; Saving:
;  CP0, R0 - 15
;  CP1, R2, R3, R8 - 11
;
;  NOTE:  This routine was written to optimize both the read performance and
;  the register usage.  It was designed against the EAS stated instruction
;  latencies for tmrc and tmrrc instructions in hopes of keeping the
;  connection between the coprocessor and core maximized with respect to bandwitdh.
;  As such, the registes are not saved in a completely linear fashion, they are
;  interleaved.  As a result, the restore must treat these like a stack and pop
;  off the registers in a similar order.
;
;******************************************************************************
Xllp_Store_All_WMMX_Regs FUNCTION

        stmdb sp!, {r0 - r11, r14}         ; Store registers to the stack so we don't munge anything

        ;Save pointer comes in R0, move to R10
        mov r10, r0

        tmrc   r0, wC2                    ;CP1, Reg2 -> Core, R0
        tmrc   r1, wC3                    ;CP1, Reg3 -> Core, R1
        tmrrc  r2, r3, wR0                ;CP0, Reg0 -> Core, R2 (Lo) R3 (High)

        ;Now store to location referenced by R10, post-increment by 4 bytes
        str    r0, [r10],#4
        str    r1, [r10],#4
        tmrrc  r4, r5, wR1
        str    r2, [r10], #4              ;Now store wR0
        str    r3, [r10], #4
        tmrrc  r0, r1, wR2
        str    r4, [r10], #4              ;Store wR1
        str    r5, [r10], #4
        tmrc   r2, wC8                    ;CP1, Reg8 -> Core, R2
        tmrrc  r6, r7, wR3
        str    r0, [r10], #4              ;Store wR2
        str    r1, [r10], #4
        tmrrc  r4, r5, wR4
        str    r2, [r10], #4              ;Store wC3
        tmrrc  r8, r9, wR5
        str    r6, [r10], #4              ;Store wR3
        str    r7, [r10], #4
        tmrrc  r2, r3, wR6
        str    r4, [r10], #4              ;Store wR4
        str    r5, [r10], #4
        tmrrc  r0, r1, wR7
        str    r8, [r10], #4              ;Store wR5
        str    r9, [r10], #4
        tmrrc  r6, r7, wR8
        str    r2, [r10], #4              ;Store wR6
        str    r3, [r10], #4
        tmrrc  r4, r5, wR9
        str    r0, [r10], #4              ;Store wR7
        str    r1, [r10], #4
        tmrrc  r8, r9, wR10
        str    r6, [r10], #4              ;Store wR8
        str    r7, [r10], #4
        tmrrc  r2, r3, wR11
        str    r4, [r10], #4              ;Store wR9
        str    r5, [r10], #4
        tmrrc  r0, r1, wR12
        str    r8, [r10], #4              ;Store wR10
        str    r9, [r10], #4
        tmrrc  r6, r7, wR13
        str    r2, [r10], #4              ;Store wR11
        str    r3, [r10], #4
        tmrrc  r4, r5, wR14
        str    r0, [r10], #4              ;Store wR12
        str    r1, [r10], #4
        tmrrc  r8, r9, wR15
        str    r6, [r10], #4              ;Store wR13
        str    r7, [r10], #4
        tmrc   r2, wC9
        str    r4, [r10], #4              ;Store wR14
        str    r5, [r10], #4
        tmrc   r0, wC10
        str    r8, [r10], #4              ;Store wR15
        str    r9, [r10], #4
        tmrc   r1, wC11
        str    r2, [r10], #4              ;Store wC9
        str    r0, [r10], #4              ;Store wC10
        str    r1, [r10], #4              ;Store wC11

        ; Now clear the control MUP & CUP bits (Control Update Bits)
        ; These are WRITE 1 TO CLEAR!
        mov     r3, #0x3                  ;Set the 2 lowest bits == 1
        tmcr    wC1, r3                   ;Now Clear the CUP & MUP bits

        ;CPWAIT  r0
        MRC     P15, 0, r0, C2, C0, 0     ; arbitrary read of CP15
        MOV     r2, r0                    ; wait for it (foward dependency)
        SUB     PC, PC, #4                ; branch to next instruction

        ldmia     sp!, {r0 - r11, r14}     ;Now restore the regsiters we stacked

        IF Interworking :LOR: Thumbing
            bx  lr
        ELSE
            mov  pc, lr          ; return
        ENDIF ; IF Interworking :LOR: Thumbing

        ENDFUNC

;******************************************************************************
;
;       *******************************
;       *                             *
;       * Xllp_Restore_All_WMMX_Regs  *
;       *                             *
;       *******************************
;
;
;
; Restores all WMMX Registers saved by the above Store function
; Restoring:
;  CP0, R0 - 15
;  CP1, R2, R3, R8 - 11
;
;  NOTE:  This routine was written to optimize both the read performance and
;  the register usage.  It was designed against the EAS stated instruction
;  latencies for tmcr and tmcrr instructions in hopes of keeping the
;  connection between the coprocessor and core maximized with respect to bandwitdh.
;  As such, the registes are not saved in a completely linear fashion, they are
;  interleaved.  Due to the order saved, the restore order is also not sequential.
;
;******************************************************************************
Xllp_Restore_All_WMMX_Regs FUNCTION

        stmdb sp!, {r0 - r11, r14}         ;Store registers to the stack so we don't munge anything

        ;Save pointer comes in R0, move to R10
        mov r10, r0

        ldr    r0, [r10], #4              ;Load wC2
        ldr    r1, [r10], #4              ;Load wC3
        ldr    r2, [r10], #4              ;Load wR0
        ldr    r3, [r10], #4
        tmcr   wC2, r0
        tmcr   wC3, r1
        ldr    r0, [r10], #4              ;Load wR1
        ldr    r1, [r10], #4
        tmcrr  wR0, r2, r3
        ldr    r2, [r10], #4              ;Load wR2
        ldr    r3, [r10], #4
        tmcrr  wR1, r0, r1
        ldr    r0, [r10], #4              ;Load wC8
        ldr    r4, [r10], #4              ;Load wR3
        ldr    r5, [r10], #4
        tmcrr  wR2, r2, r3
        ldr    r6, [r10], #4              ;Load wR4
        ldr    r7, [r10], #4
        tmcr   wC8, r0
        ldr    r0, [r10], #4              ;Load wR5
        ldr    r1, [r10], #4
        tmcrr  wR3, r4, r5
        ldr    r2, [r10], #4              ;Load wR6
        ldr    r3, [r10], #4
        tmcrr  wR4, r6, r7
        ldr    r4, [r10], #4              ;Load wR7
        ldr    r5, [r10], #4
        tmcrr  wR5, r0, r1
        ldr    r0, [r10], #4              ;Load wR8
        ldr    r1, [r10], #4
        tmcrr  wR6, r2, r3
        ldr    r2, [r10], #4              ;Load wR9
        ldr    r3, [r10], #4
        tmcrr  wR7, r4, r5
        ldr    r4, [r10], #4              ;Load wR10
        ldr    r5, [r10], #4
        tmcrr  wR8, r0, r1
        ldr    r0, [r10], #4              ;Load wR11
        ldr    r1, [r10], #4
        tmcrr  wR9, r2, r3
        ldr    r2, [r10], #4              ;Load wR12
        ldr    r3, [r10], #4
        tmcrr  wR10, r4, r5
        ldr    r4, [r10], #4              ;Load wR13
        ldr    r5, [r10], #4
        tmcrr  wR11, r0, r1
        ldr    r0, [r10], #4              ;Load wR14
        ldr    r1, [r10], #4
        tmcrr  wR12, r2, r3
        ldr    r2, [r10], #4              ;Load wR15
        ldr    r3, [r10], #4
        tmcrr  wR13, r4, r5
        ldr    r4, [r10], #4              ;Load wC9
        tmcrr  wR14, r0, r1
        ldr    r5, [r10], #4              ;Load wC10
        tmcrr  wR15, r2, r3
        ldr    r0, [r10], #4              ;Load wC11
        tmcr   wC9, r4
        tmcr   wC10, r5
        tmcr   wC11, r0

        ; Now clear the control MUP & CUP bits (Control Update Bits)
        ; These are WRITE 1 TO CLEAR!
        mov     r1, #0x3                  ;Set the 2 lowest bits == 1
        tmcr    wC1, r1                   ;Now Clear the CUP & MUP bits

        ;CPWAIT  r2
        MRC     P15, 0, r2, C2, C0, 0     ; arbitrary read of CP15
        MOV     r2, r2                    ; wait for it (foward dependency)
        SUB     PC, PC, #4                ; branch to next instruction

        ldmia     sp!, {r0 - r11, r14}     ;Now restore the regsiters we stacked

        IF Interworking :LOR: Thumbing
            bx  lr
        ELSE
            mov  pc, lr          ; return
        ENDIF ; IF Interworking :LOR: Thumbing


        ENDFUNC


        END
